/******************************************************************************
 * Copyright 2022 The AIR Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "v2x-asn-msgs-internal.h"

static bool motion_asn2pb(const MotionConfidenceSet_t *asn_motion,
                          ::v2xpb::asn::MotionConfidenceSet *pb_motion) {
  if (!asn_motion || !pb_motion) {
    LOG(WARNING) << "args false";
    return false;
  }
  if (asn_motion->speedCfd) {
    pb_motion->set_speed_cfd(static_cast<int>(*asn_motion->speedCfd));
  }
  if (asn_motion->headingCfd) {
    pb_motion->set_heading_cfd(static_cast<int>(*asn_motion->headingCfd));
  }
  if (asn_motion->steerCfd) {
    pb_motion->set_steer_cfd(static_cast<int>(*asn_motion->steerCfd));
  }
  return true;
}

static bool motion_pb2asn(const ::v2xpb::asn::MotionConfidenceSet &pb_motion,
                          MotionConfidenceSet_t *asn_motion) {
  if (!asn_motion) {
    LOG(WARNING) << "args false";
    return false;
  }
  if (pb_motion.has_speed_cfd()) {
    asn_motion->speedCfd = ASN1(SpeedConfidence)::create_tp();
    *asn_motion->speedCfd = pb_motion.speed_cfd();
  }
  if (pb_motion.has_heading_cfd()) {
    asn_motion->headingCfd = ASN1(HeadingConfidence)::create_tp();
    *asn_motion->headingCfd = pb_motion.heading_cfd();
  }
  if (pb_motion.has_steer_cfd()) {
    asn_motion->steerCfd = ASN1(SteeringWheelAngleConfidence)::create_tp();
    *asn_motion->steerCfd = pb_motion.steer_cfd();
  }
  return true;
}

static bool accel_asn2pb(const AccelerationSet4Way_t *asn_accel,
                         ::v2xpb::asn::AccelerationSet4Way *pb_accel) {
  if (!asn_accel || !pb_accel) {
    LOG(WARNING) << "args false";
    return false;
  }
  pb_accel->set_lon(static_cast<int>(asn_accel->Long));
  pb_accel->set_lat(static_cast<int>(asn_accel->lat));
  pb_accel->set_vert(static_cast<int>(asn_accel->vert));
  pb_accel->set_yaw(static_cast<int>(asn_accel->yaw));
  return true;
}

static bool accel_pb2asn(const ::v2xpb::asn::AccelerationSet4Way &pb_accel,
                         AccelerationSet4Way_t *asn_accel) {
  if (!asn_accel) {
    LOG(WARNING) << "args false";
    return false;
  }
  if (pb_accel.has_lon()) {
    asn_accel->Long = pb_accel.lon();
  }
  if (pb_accel.has_lat()) {
    asn_accel->lat = pb_accel.lat();
  }
  if (pb_accel.has_vert()) {
    asn_accel->vert = pb_accel.vert();
  }
  if (pb_accel.has_yaw()) {
    asn_accel->yaw = pb_accel.yaw();
  }
  return true;
}

static bool vehicle_size_asn2pb(const VehicleSize_t &asn_vsize,
                                ::v2xpb::asn::VehicleSize *pb_vsize) {
  if (!pb_vsize) {
    LOG(WARNING) << "args false";
    return false;
  }
  pb_vsize->set_width(static_cast<int>(asn_vsize.width));
  pb_vsize->set_length(static_cast<int>(asn_vsize.length));
  if (asn_vsize.height) {
    pb_vsize->set_height(static_cast<int>(*asn_vsize.height));
  }
  return true;
}

static bool vehicle_size_pb2asn(const ::v2xpb::asn::VehicleSize &pb_vsize,
                                VehicleSize_t *asn_vsize) {
  if (pb_vsize.has_width()) {
    asn_vsize->width = pb_vsize.width();
  }
  if (pb_vsize.has_length()) {
    asn_vsize->length = pb_vsize.length();
  }
  if (pb_vsize.has_height()) {
    asn_vsize->height = ASN1(VehicleHeight)::create_tp();
    *asn_vsize->height = pb_vsize.height();
  }
  return true;
}

/////
bool rsm_asn2pb(const RoadsideSafetyMessage_t *rsm_asn,
                ::v2xpb::asn::Rsm *rsm_pb) {
  if (!rsm_asn || !rsm_pb) {
    LOG(INFO) << "args is false";
    return false;
  }
  rsm_pb->set_msg_cnt(static_cast<int>(rsm_asn->msgCnt));
  if (rsm_asn->id.buf) {
    rsm_pb->set_id(
        std::string((const char *)(rsm_asn->id.buf), rsm_asn->id.size));
  }
  position_asn2pb(&rsm_asn->refPos, rsm_pb->mutable_refpos());

  for (int i = 0; i < rsm_asn->participants.list.count; i++) {
    const auto asn_pant = rsm_asn->participants.list.array[i];
    auto pb_pant = rsm_pb->add_participants();

    pb_pant->set_ptc_type((::v2xpb::asn::ParticipantType)asn_pant->ptcType);
    pb_pant->set_ptc_id(static_cast<int>(asn_pant->ptcId));
    pb_pant->set_source((::v2xpb::asn::SourceType)asn_pant->source);
    if (asn_pant->id && asn_pant->id->buf) {
      pb_pant->set_id(
          std::string((const char *)(asn_pant->id->buf), asn_pant->id->size));
    }

    position_asn2pb(&rsm_asn->refPos, &asn_pant->pos, pb_pant->mutable_pos());
    pb_pant->mutable_pos_confidence()->set_pos(asn_pant->posConfidence.pos);
    if (asn_pant->posConfidence.elevation) {
      pb_pant->mutable_pos_confidence()->set_elevation(
          static_cast<int>(*asn_pant->posConfidence.elevation));
    }
    if (asn_pant->transmission) {
      pb_pant->set_transmission(static_cast<int>(*asn_pant->transmission));
    }
    pb_pant->set_speed(static_cast<int>(asn_pant->speed));
    pb_pant->set_heading(static_cast<int>(asn_pant->heading));

    if (asn_pant->angle) {
      pb_pant->set_angle(static_cast<int>(*asn_pant->angle));
    }
    if (asn_pant->motionCfd) {
      motion_asn2pb(asn_pant->motionCfd, pb_pant->mutable_motion_cfd());
    }
    if (asn_pant->accelSet) {
      accel_asn2pb(asn_pant->accelSet, pb_pant->mutable_accel_set());
    }
    vehicle_size_asn2pb(asn_pant->size, pb_pant->mutable_size());
    if (asn_pant->vehicleClass) {
      pb_pant->mutable_vehicle_class()->set_classification(
          static_cast<int>(asn_pant->vehicleClass->classification));
      if (asn_pant->vehicleClass->fuelType) {
        pb_pant->mutable_vehicle_class()->set_fuel_type(
            static_cast<int>(*asn_pant->vehicleClass->fuelType));
      }
    }
  }

  return true;
}

bool rsm_pb2asn(const ::v2xpb::asn::Rsm &rsm_pb,
                RoadsideSafetyMessage_t *rsm_asn) {
  if (!rsm_asn) {
    LOG(INFO) << "args is false";
    return false;
  }

  rsm_asn->msgCnt = rsm_pb.msg_cnt();
  OCTET_STRING_fromBuf(&rsm_asn->id, rsm_pb.id().c_str(), rsm_pb.id().size());
  position_pb2asn(rsm_pb.refpos(), &rsm_asn->refPos);

  for (const auto &pb_pant : rsm_pb.participants()) {
    auto asn_pant = ASN1(ParticipantData)::create_tp();
    ASN_SEQUENCE_ADD(&rsm_asn->participants.list, asn_pant);

    asn_pant->ptcType = (ParticipantType)pb_pant.ptc_type();
    asn_pant->ptcId = pb_pant.ptc_id();
    asn_pant->source = (SourceType)pb_pant.source();

    if (pb_pant.has_id()) {
      OCTET_STRING_fromBuf(asn_pant->id, pb_pant.id().c_str(),
                           pb_pant.id().size());
    }

    if (pb_pant.has_pos_confidence()) {
      asn_pant->posConfidence.pos = pb_pant.pos_confidence().pos();
      if (pb_pant.pos_confidence().has_elevation()) {
        asn_pant->posConfidence.elevation =
            ASN1(ElevationConfidence)::create_tp();
        *asn_pant->posConfidence.elevation =
            pb_pant.pos_confidence().elevation();
      }
    }

    asn_pant->secMark = pb_pant.sec_mark();
    position_pb2asn(&rsm_asn->refPos, pb_pant.pos(), &asn_pant->pos);

    if (pb_pant.has_transmission()) {
      asn_pant->transmission = ASN1(TransmissionState)::create_tp();
      *asn_pant->transmission = pb_pant.transmission();
    }

    if (pb_pant.has_speed()) {
      asn_pant->speed = pb_pant.speed();
    }

    if (pb_pant.has_heading()) {
      asn_pant->speed = pb_pant.heading();
    }

    if (pb_pant.has_angle()) {
      asn_pant->angle = ASN1(SteeringWheelAngle)::create_tp();
      *asn_pant->angle = pb_pant.angle();
    }

    if (pb_pant.has_motion_cfd()) {
      asn_pant->motionCfd = ASN1(MotionConfidenceSet)::create_tp();
      motion_pb2asn(pb_pant.motion_cfd(), asn_pant->motionCfd);
    }

    if (pb_pant.has_accel_set()) {
      asn_pant->accelSet = ASN1(AccelerationSet4Way)::create_tp();
      accel_pb2asn(pb_pant.accel_set(), asn_pant->accelSet);
    }

    if (pb_pant.has_size()) {
      vehicle_size_pb2asn(pb_pant.size(), &asn_pant->size);
    }

    if (pb_pant.has_vehicle_class()) {
      asn_pant->vehicleClass = ASN1(VehicleClassification)::create_tp();
      if (pb_pant.vehicle_class().has_classification()) {
        asn_pant->vehicleClass->classification =
            pb_pant.vehicle_class().classification();
      }
      if (pb_pant.vehicle_class().has_fuel_type()) {
        asn_pant->vehicleClass->fuelType = ASN1(FuelType)::create_tp();
        *asn_pant->vehicleClass->fuelType = pb_pant.vehicle_class().fuel_type();
      }
    }
  }

  return true;
}
